/*
* @file    mainwindow.cpp
* @author  Alberto Rubin
*/

#include "registration.h"
#include "selectComponent.h"
#include "coplanarity.h"

#include <QtGui>
#include "mainwindow.h"
#include <qpushbutton.h>

#define SEG_TH 1.2 /*!< Default segmentation threshold. */
#define FILTER_TH 2 /*!< Default filtering threshold. */

QDialog *n, *o, *ardoran;
QLineEdit *th1, *th2;
int angle = COPLANARITY_TH;
bool selection = false, cons = true, presence = true, rib = true;
float th_filter, th_seg;
QRadioButton *radio1;

/*! \defgroup group1 Point Cloud Group
  Global variables rappresenting point clouds.

  @{
*/

pcl::PointCloud<pcl::PointXYZRGB>::Ptr source; /*!< source point cloud. */
pcl::PointCloud<pcl::PointXYZRGB>::Ptr target; /*!< target point cloud. */
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_segmented; /*!< source segmented point cloud. */
pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_segmented; /*!< target segmented point cloud. */
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_registered; /*!< source registered point cloud. */
pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp_target_segmented; /*!< temp point cloud. */

/*! @} */ // end of group1

boost::signals2::connection componentCallbackConnection; /*!< callback connection for component selection. */
boost::signals2::connection componentPresenceCallbackConnection; /*!< callback connection for component presence check. */
boost::signals2::connection componentCallbackCheckRibbonConnection; /*!< callback connection for ribbon cable check. */


MainWindow::MainWindow(){

	//Define Point Cloud.
	source.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	target.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	source_segmented.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	target_segmented.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	source_registered.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	temp_target_segmented.reset(new pcl::PointCloud<pcl::PointXYZRGB>);

	//Define PCL Visualizer.
	viewer = new pcl::visualization::PCLVisualizer("PCL Visualizer", false);

	setWindowTitle("Quality checker");
	resize(1000, 800);
	QMainWindow::showMaximized();

	QRect available_geom = QDesktopWidget().availableGeometry();
	QRect current_geom = frameGeometry();
	setGeometry(available_geom.width() / 2 - current_geom.width() / 2,
	                        available_geom.height() / 2 - current_geom.height() / 2,
	                        current_geom.width(),
	                        current_geom.height());

	createActions();
	createMenus();
	createToolBars();
	createStatusBar();

	label = new QLabel(tr("Select Point Cloud:"), this);
	title = new QLabel(tr(""), this);
	title->setFont(QFont("Purisa"));
	QPalette p0 = title->palette();
	p0.setColor(QPalette::WindowText, Qt::red);
	title->setPalette(p0);

	//Coplanarity check
	label2 = new QLabel(tr("Select component to check coplanarity:"), this);
	checkCoplanarity = new QPushButton(tr("Coplanarity"), this);
	checkCoplanarity->setFixedWidth(150);
	checkCoplanarity->setEnabled(false);

	angleTollerance = new QSpinBox();
	angleTollerance->setMinimum(1);
	angleTollerance->setMaximum(360);
	angleTollerance->setEnabled(false);


	bgroup = new QButtonGroup;

	radio1 = new QRadioButton(tr("Presence"));
	radio2 = new QRadioButton(tr("Coplanarity"));
	radio3 = new QRadioButton(tr("Other"));

	bgroup->addButton(radio1);
	bgroup->addButton(radio2);
	bgroup->addButton(radio3);

	QVBoxLayout *vbox = new QVBoxLayout;

	QHBoxLayout *hradio1 = new QHBoxLayout;
	QHBoxLayout *hradio2 = new QHBoxLayout;
	QHBoxLayout *hradio3 = new QHBoxLayout;

	hradio1->addWidget(radio1);

	hradio2->addWidget(radio2);
	hradio2->addWidget(angleTollerance);
	hradio2->addWidget(checkCoplanarity);

	hradio3->addWidget(radio3);

	vbox->addLayout(hradio1);
	vbox->addLayout(hradio2);
	vbox->addLayout(hradio3);

	checkbox = new QCheckBox("check box");

	//Combobox per la selezione delle visualizzazione
	selVis = new QComboBox();
	selVis->insertItem(0, "source");
	selVis->insertItem(1, "target");
	selVis->insertItem(2, "source segmented");
	selVis->insertItem(3, "target segmented");
	selVis->insertItem(4, "source registered");

	selVis->setEditable(true);
	selVis->lineEdit()->setAlignment(Qt::AlignHCenter);
	selVis->lineEdit()->setReadOnly(true);
	for(int i = 0; i < selVis->count(); i++)
		selVis->setItemData(i, Qt::AlignHCenter, Qt::TextAlignmentRole);

	for(int i = 0; i < selVis->count(); i++ )
		setCombobox(i, false);

	//Visualizzatore point cloud
	qvtkVisualizer = new QVTKWidget();
	qvtkVisualizer->resize(450, 300);
	qvtkVisualizer->setMinimumSize(350, 200);
	init();

	//Console per scrittura messaggi
	console = new QTextEdit();
	console->setReadOnly(true);

	QPalette p = console->palette();
	p.setColor(QPalette::Base, Qt::black);
	console->setPalette(p);
	console->setTextColor(Qt::white);

	console->setText("Benvenuto in Quality Checker. \n");
	console->resize(450, 50);
	scrollH = new QScrollBar;
	scrollV = new QScrollBar;
	console->setHorizontalScrollBar(scrollH);
	console->setHorizontalScrollBarPolicy(Qt::ScrollBarAsNeeded);
	console->setVerticalScrollBar(scrollV);
	console->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded);

	//ASCOLTATORI
	connect(selVis, SIGNAL(currentIndexChanged(int)), this, SLOT(selectVis(int)));
	connect(checkCoplanarity, SIGNAL(clicked()), this, SLOT(coplanarityCheck()));
	connect(bgroup, SIGNAL(buttonClicked(int)), this, SLOT(chooseCheck(int)));
	connect(angleTollerance, SIGNAL(valueChanged(int)), this, SLOT(updateAngleTollerance(int)));

	//LAYOUT

	//Style Sheet per impostare il layout dei QGroupBox
	this->setStyleSheet("QGroupBox { border: 2px solid black; border-radius: 5px; margin-top: 7px; margin-bottom: 7px; padding: 0px } "
			"QGroupBox::title { subcontrol-origin: border; subcontrol-position: top left ; padding: 10 10 10 10 ; left: 10px ; top: -17px}");

	QHBoxLayout *topLeftLayout2 = new QHBoxLayout();
	topLeftLayout2->addWidget(label);
	topLeftLayout2->addWidget(selVis);
	QHBoxLayout *topLeftLayout3 = new QHBoxLayout();
	//topLeftLayout3->addWidget(selComponent);
	//topLeftLayout3->addWidget(newWind);
	topLeftLayout3->addLayout(vbox);

	QGroupBox *f1 = new QGroupBox("Visualize Point Cloud");
	f1->setLayout(topLeftLayout2);

	/*
	QFrame *f = new QFrame();
	//f->setFrameStyle(QFrame::StyledPanel);
	f->setFrameStyle(QFrame::WinPanel);
	f->setFrameShadow(QFrame::Raised);
	f->setLineWidth(3);
	f->setMidLineWidth(3);
	f->setLayout(topLeftLayout2);
	f->setFixedHeight(100);
	QPalette *pf = new QPalette();
	pf->setColor(QPalette::Foreground, Qt::black);
	f->setPalette(*pf);
	*/

	/*QFrame *f2 = new QFrame();
	//f->setFrameStyle(QFrame::StyledPanel);
	f2->setFrameStyle(QFrame::WinPanel);
	f2->setFrameShadow(QFrame::Raised);
	f2->setLineWidth(3);
	f2->setMidLineWidth(3);
	f2->setLayout(topLeftLayout3);
	f2->setFixedHeight(100);
	QPalette *pf2 = new QPalette();
	pf2->setColor(QPalette::Foreground, Qt::black);
	f2->setPalette(*pf2);*/

	
    f2 = new QGroupBox("Check");
	f2->setLayout(topLeftLayout3);

	QHBoxLayout *topLeftLayout5 = new QHBoxLayout();
	topLeftLayout5->addWidget(label2);
	topLeftLayout5->addWidget(checkbox);*/
	
	

	/*QFrame *f3 = new QFrame();
	//f->setFrameStyle(QFrame::StyledPanel);
	f3->setFrameStyle(QFrame::WinPanel);
	f3->setFrameShadow(QFrame::Raised);
	f3->setLineWidth(3);
	f3->setMidLineWidth(3);
	f3->setLayout(topLeftLayout5);
	f3->setFixedHeight(100);
	QPalette *pf3 = new QPalette();
	pf3->setColor(QPalette::Foreground, Qt::black);
	f3->setPalette(*pf3);*/

	QGroupBox *f3 = new QGroupBox("Check2");
	f3->setLayout(topLeftLayout5);

	QVBoxLayout *topLeftLayout4 = new QVBoxLayout();
	topLeftLayout4->addWidget(f1);
	topLeftLayout4->addWidget(f2);

	QVBoxLayout *topRightLayout = new QVBoxLayout();
	topRightLayout->addWidget(qvtkVisualizer);
	topRightLayout->setContentsMargins(0,0,10,0);

	QLabel *emptyLabel = new QLabel(" ");
	QHBoxLayout *t1 = new QHBoxLayout();
	t1->addWidget(emptyLabel);
	t1->addWidget(title);
	t1->setContentsMargins(2, 2, 80, 2);

	QHBoxLayout *down = new QHBoxLayout();
	down->addLayout(topLeftLayout4);
	down->addLayout(topRightLayout);

	QVBoxLayout *mainLayout = new QVBoxLayout();
	mainLayout->addLayout(t1);
	mainLayout->addLayout(down);

	QVBoxLayout *prova = new QVBoxLayout();
	prova->addLayout(mainLayout);
	console->setFixedHeight(200);
	prova->addWidget(console);

	QWidget *widCentrale = new QWidget();
	widCentrale->setLayout(prova);
	setCentralWidget(widCentrale);

	resetAll();

}


void MainWindow::visualize(pcl::PointCloud<pcl::PointXYZRGB>::Ptr v, QString s){

	//title->setText("<h2><center>" + s.toUpper() + "</center></h2>");

	if(v->height*v->width == 0){
		QMessageBox::critical(this, tr("Error"), tr("Empty Point Cloud"));
		return;
	}

	QStringList temp = s.split("_");
	QString name = "";
	for(int i = 0; i < temp.size(); i++ )
		name += temp[i] + " ";

	title->setText("<h2><center>" + name.toUpper() + "</center></h2>");

	viewer->removeAllPointClouds();

	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(v);
	viewer->addPointCloud<pcl::PointXYZRGB> (v, rgb, s.toStdString());
	viewer->resetCamera();
	qvtkVisualizer->update();
	//sendConsole("Visualize: " + s + "\n");

}


void MainWindow::openFile(){

	QString old = fileName;
	fileName = QFileDialog::getOpenFileName(this, tr("Open File"), "/home", tr("PCD (*.pcd)"));

	if(fileName == ""){
		QMessageBox::critical(this, tr("Error"), tr("Select a file to open"));
		fileName = old;
		return;
	}

	resetAll();

	pcl::io::loadPCDFile (fileName.toStdString(), *source);
	QStringList s = fileName.split("/");
	sendConsole("Source " + s[ s.size()-1 ] + " loaded. \n");

	setCombobox(0, true);

	for(int i = 1; i < selVis->count(); i++ )
		setCombobox(i, false);

	selVis->setCurrentIndex(0);
	visualize(source, "source");


}

void MainWindow::openTarget(){

	target->clear();
	target_segmented->clear();

	QString targetName = "";
	targetName = QFileDialog::getOpenFileName(this, tr("Open Target"), "/home", tr("PCD (*.pcd)"));

	if(targetName == ""){
		QMessageBox::critical(this, tr("Error"), tr("Select a file to open"));
		return;
	}

	pcl::io::loadPCDFile (targetName.toStdString(), *target);
	QStringList s = targetName.split("/");
	sendConsole("Target " + s[ s.size()-1 ] + " loaded. \n");

	setCombobox(1, true);

	for(int i = 2; i < selVis->count(); i++ )
		setCombobox(i, false);

	selVis->setCurrentIndex(1);
	visualize(target, "target");


}


void MainWindow::selectVis(int index){

	resetPresence();
	resetCoplanarity();

	switch(index){

		case 0:
			visualize(source, "source");
			break;

		case 1:
			visualize(target, "target");
			break;

		case 2:
			visualize(source_segmented, "source_segmented");
			break;

		case 3:
			visualize(target_segmented, "target_segmented");
			break;

		case 4:
			visualize(source_registered, "source_registered");
			break;

		default:
			visualize(source, "source");
			break;
	}

}

void MainWindow::segment(){

	if(source->width*source->height == 0){
		QMessageBox::critical(this, tr("Error"), tr("Select a source first"));
		return;
	}

	if(target->width*target->height == 0){
		QMessageBox::critical(this, tr("Error"), tr("Select a target first"));
		return;
	}

	source_segmented->clear();
	target_segmented->clear();

	pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_vox (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_vox (new pcl::PointCloud<pcl::PointXYZRGB>);

	sendConsole("Segmentation...");

	/*voxelCloud(source, source_vox, 0.1);
	voxelCloud(target, target_vox, 0.1);

	segmentation(source_vox, source_segmented, th_seg);
	segmentation(target_vox, target_segmented, th_seg);*/

	segmentation(source, source_segmented, th_seg);
	segmentation(target, target_segmented, th_seg);
	sendConsole("OK. \n");

	for(int i = 0; i < 4; i++ )
		setCombobox(i, true);

	selVis->setCurrentIndex(2);
	visualize(source_segmented, "source_segmented");

}

void MainWindow::registers(){

	if(source_segmented->width*source_segmented->height == 0){
		QMessageBox::critical(this, tr("Error"), tr("Segment a source first"));
		return;
	}

	source_registered->clear();

	pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints (new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints (new pcl::PointCloud<pcl::PointXYZI>);

	pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);

	//CALCOLO DEI KEYPOINTS con SIFT3D
	sendConsole("Source keypoints creation...");
	computeKeyPoints(source_segmented, source_keypoints);
	sendConsole("OK \n");

	sendConsole("Target keypoints creation... ");
	computeKeyPoints(target_segmented, target_keypoints);
	sendConsole("OK \n");

	//ESTRAZIONI DEI DESCRITTORI (feature)
	sendConsole("Features extraction...");
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_f (new pcl::PointCloud<pcl::FPFHSignature33>);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_f (new pcl::PointCloud<pcl::FPFHSignature33>);
	extractFeatures(source_segmented, source_keypoints, source_f);
	extractFeatures(target_segmented, target_keypoints, target_f);
	sendConsole("OK \n");

	//TROVARE LE CORRISPONDENZE e FILTRARLE
	std::vector<int> s2t;
	std::vector<int> t2s;

	findCorrespondences(source_f,target_f,s2t);
	findCorrespondences(target_f,source_f,t2s);

	//FILTRAGGIO
	pcl::CorrespondencesPtr corrispondenzePtr (new pcl::Correspondences);
	std::vector<std::pair<unsigned, unsigned> > corrispondenze;
	sendConsole("Filtering correspondences...");
	filterCorrespondences(s2t, t2s, corrispondenzePtr, corrispondenze, source_keypoints, target_keypoints, th_filter);
	sendConsole("OK \n");

	//DETERMINARE L'ALLINEAMENTO INIZIALE
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_trasformata (new pcl::PointCloud<pcl::PointXYZRGB>);

	Eigen::Matrix4f initial_transformation_matrix;
	sendConsole("Initial alignment...");
	initialAlignment(corrispondenzePtr, source_keypoints, target_keypoints, source_segmented, source_trasformata, initial_transformation_matrix);
	sendConsole("OK \n");

	//DETERMINARE L'ALLINEAMENTO FINALE
	Eigen::Matrix4f transformation_matrix;
	clock_t start,end;
	double tempo;
	start=clock();
	//finalAlignment(source_trasformata, target, source_registered, transformation_matrix);
	sendConsole("Final alignment...");
	finalAlignment(source_trasformata, target_segmented, source_registered, transformation_matrix);
	end=clock();
	tempo=((double)(end-start))/CLOCKS_PER_SEC;
	sendConsole("OK \n");

	sendConsole("Registration complete in " + QString::number(tempo, 'f', 3) + " seconds... \n");

	startSelectionAct->setEnabled(true);
	selVis->setCurrentIndex(4);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr unione (new pcl::PointCloud<pcl::PointXYZRGB>);

	(*unione) = (*source_registered) + (*target_segmented);

	setCombobox(4, true);
	startSelectionAct->setEnabled(true);
	stopSelectionAct->setEnabled(false);
	presence = true;

	/*
	//PROVO A VOXELIZZARE DOPO LA REGISTRAZIONE PER VELOCIZZARE LE OPERAZIONI
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_registered_temp (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_segmented_temp (new pcl::PointCloud<pcl::PointXYZRGB>);

	voxelCloud(source_registered, source_registered_temp, 0.5);
	source_registered->clear();
	pcl::copyPointCloud(*source_registered_temp, *source_registered);

	voxelCloud(target_segmented, target_segmented_temp, 0.5);
	target_segmented->clear();
	pcl::copyPointCloud(*target_segmented_temp, *target_segmented);
	*/

	//visualize(source_registered, "source_registered");
	visualize(unione, "unione");


}

void MainWindow::newWindow(){

	n = new QDialog(this);
	n->setFixedSize(800, 640);

	QVTKWidget *visualizerSelection = new QVTKWidget();
	pcl::visualization::PCLVisualizer tempViewer("Visualizzatore PCL", false);

	QPushButton *save = new QPushButton("Save");
	save->setFixedWidth(200);
	connect(save, SIGNAL(clicked()), this, SLOT(saveComponent()));

	QPushButton *cancel = new QPushButton("Cancel");
	cancel->setFixedWidth(200);
	connect(cancel, SIGNAL(clicked()), n, SLOT(close()));

	tempViewer.setBackgroundColor (0.5, 0.5, 0.5);
	tempViewer.addCoordinateSystem (0.1);
	tempViewer.initCameraParameters ();
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(getSelectedComponent());
	tempViewer.removeAllPointClouds();
	tempViewer.addPointCloud<pcl::PointXYZRGB> (getSelectedComponent(), rgb, "component");
	tempViewer.resetCamera();

	vtkSmartPointer<vtkRenderWindow> renderWindow = tempViewer.getRenderWindow();
	visualizerSelection->SetRenderWindow(renderWindow);
	tempViewer.setupInteractor(visualizerSelection->GetInteractor(), visualizerSelection->GetRenderWindow());

	QVBoxLayout *l = new QVBoxLayout();
	l->addWidget(visualizerSelection);
	QHBoxLayout *buttonLayout = new QHBoxLayout();
	buttonLayout->addWidget(save);
	buttonLayout->addWidget(cancel);
	l->addLayout(buttonLayout);

	n->setLayout(l);
	l->deleteLater();
	n->exec();

}

void MainWindow::selectComponent(){

	if (!selection){

		visualize(source_registered, "source_registered");

		componentCallbackConnection = viewer->registerPointPickingCallback(&pointPickCallback, this);
		selection = true;
	}
	else{

		componentCallbackConnection.disconnect();
		saveAct->setEnabled(true);
		selection = false;
	}

}

void MainWindow::saveComponent(){

	QString fileName = QFileDialog::getSaveFileName(this,
	         tr("Save Component"), "",
	         tr("PCD (*.pcd)"));
	if (fileName.isEmpty()){
		QMessageBox::critical(this, tr("Error"), tr("Insert file name"));
		return;
	}

	QStringList s = fileName.split("/");
	QStringList t = (s[ s.size() - 1 ]).split(".");
	if(t[ t.size() - 1].compare("pcd") != 0)
		fileName.append(".pcd");

	pcl::io::savePCDFile(fileName.toStdString(), *(getSelectedComponent()));
	n->close();

}

void MainWindow::about(){

	QMessageBox::about(this, tr("About Application"), tr("The <b>Application</b> is powered by Petrucci Rossella, Rigo Alvise & Rubin Alberto "));

}

void MainWindow::coplanarityCheck(){


	/*
		coplanarity() prende in input la cloud completa (scheda + tavolo ) e il componente già isolato da verificare.
		Quello che fa è segmentare il tavolo e il componente per ottenere le coordinate dei piani
		che li descrivono (i coefficienti a,b,c,d).
	*/

	if(source->width*source->height == 0 ){
		QMessageBox::warning(this, tr("Error"), tr("Load a source point cloud first"));
		return;
	}

	if(getSelectedComponent()->width*getSelectedComponent()->height == 0 ){
		QMessageBox::warning(this, tr("Error"), tr("Select a component first"));
		return;
	}

	if(coplanarity(source, getSelectedComponent(), false, angle)) // true per stampare i valori dei coefficienti.
		sendConsole("**** Selected component is coplanar! **** \n");
	else
		sendConsole("**** Selected component is not coplanar! **** \n");

}

void MainWindow::options(){

	o = new QDialog(this);
	o->setFixedSize(550, 300);

	QLabel *l1 = new QLabel("Segmentation threshold ");
	l1->setFixedWidth(250);
	th1 = new QLineEdit();
	th1->setFixedWidth(80);
	th1->setValidator(new QDoubleValidator(-1000, 1000, 3, th1));
	th1->setText(QString::number(th_seg, 'f', 3));

	QLabel *l2 = new QLabel("Registration threshold ");
	l2->setFixedWidth(250);
	th2 = new QLineEdit();
	th2->setFixedWidth(80);
	th2->setValidator(new QDoubleValidator(-1000, 1000, 3, th2));
	th2->setText(QString::number(th_filter, 'f', 3));

	//SOGLIA SEGMENTAZIONE
	QHBoxLayout *h1 = new QHBoxLayout();
	h1->addWidget(l1);
	h1->addWidget(th1);
	h1->setSpacing(5);

	//SOGLIA REGISTRAZIONE
	QHBoxLayout *h2 = new QHBoxLayout();
	h2->addWidget(l2);
	h2->addWidget(th2);
	h2->setSpacing(5);

	QPushButton *cancel = new QPushButton("Cancel");
	cancel->setFixedWidth(200);
	connect(cancel, SIGNAL(clicked()), o, SLOT(close()));

	QPushButton *done = new QPushButton("Done");
	done->setFixedWidth(200);
	connect(done, SIGNAL(clicked()), this, SLOT(confirmOptions()));

	QVBoxLayout *main = new QVBoxLayout();
	QHBoxLayout *buttonLayout = new QHBoxLayout();
	buttonLayout->addWidget(done);
	buttonLayout->addWidget(cancel);
	main->addLayout(h1);
	main->addLayout(h2);
	main->addLayout(buttonLayout);

	o->setLayout(main);
	main->deleteLater();
	o->exec();

}

void MainWindow::confirmOptions(){

	th_seg = th1->text().toFloat();
	th_filter = th2->text().toFloat();
	o->close();

}

void MainWindow::loadComponent(){

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr only_component(new pcl::PointCloud<pcl::PointXYZRGB>);

	QString old = fileName;
	fileName = QFileDialog::getOpenFileName(this, tr("Open File"), "/home", tr("PCD (*.pcd)"));

	if(fileName == ""){
		QMessageBox::critical(this, tr("Error"), tr("Select a file to open"));
		fileName = old;
		return;
	}

	getSelectedComponent()->clear();
	pcl::io::loadPCDFile (fileName.toStdString(), *(getSelectedComponent()));
	QStringList s = fileName.split("/");
	sendConsole("File " + s[s.size() -1 ] + " loaded. \n");
	QStringList name = s[s.size() -1 ].split(".");
	visualize(getSelectedComponent(), name[0]);

}

void MainWindow::cableCheck(){

	if(source->width*source->height == 0){
		QMessageBox::critical(this, tr("Error"), tr("Select a source first"));
		return;
	}

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr old_target (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr old_target_segmented (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr old_source_registered (new pcl::PointCloud<pcl::PointXYZRGB>);

	pcl::copyPointCloud( *(target), *(old_target) );
	pcl::copyPointCloud( *(target_segmented), *(old_target_segmented) );
	pcl::copyPointCloud( *(source_registered), *(old_source_registered) );

	pcl::io::loadPCDFile ( "../dataset/inliers2.pcd", *(target));

	segment();
	registers();

	resetCoplanarity();
	resetPresence();

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr area (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cavo (new pcl::PointCloud<pcl::PointXYZRGB>);

	selectCableArea(source_registered, area);

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
	transformCloud(area, cluster);

	//INDIVIDUAZIONE CAVO
	cableDetection(cluster, cavo);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr unione2 (new pcl::PointCloud<pcl::PointXYZRGB>);
	(*unione2) = (*source_registered) + (*cavo);

	//INDIVIDUAZIONE PIOLI
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr piolo1 (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr piolo2 (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr final (new pcl::PointCloud<pcl::PointXYZRGB>);

	pioliDetection(unione2, final, piolo1, piolo2);
	(*unione2) += (*final);

	findBegin(unione2);

	bool correct = finalCheck(piolo1, piolo2, cavo);

	if(correct)
		sendConsole("**** Cable is in correct position **** \n");
	else
		sendConsole("**** Cable is not in correct position **** \n");

	visualize(unione2, "source_with_cable");

	target->clear();
	target_segmented->clear();
	pcl::copyPointCloud( *(old_target), *(target) );
	pcl::copyPointCloud( *(old_target_segmented), *(target_segmented) );
	pcl::copyPointCloud( *(old_source_registered), *(source_registered) );

}


void MainWindow::presenceCheck(){

	if(presence){

		componentPresenceCallbackConnection = viewer->registerPointPickingCallback(&pointPickCallbackCheckPresence, this);
		visualize( target_segmented, "target_segment" );
		startSelectionAct->setEnabled(false);
		stopSelectionAct->setEnabled(true);
		statusBar()->showMessage(tr("Checking presence of components..."));
		radio1->setChecked(true);
		presence = false;

	}
	else{

		if(bgroup->checkedButton() != 0){
			bgroup->setExclusive(false);
			bgroup->checkedButton()->setChecked(false);
			bgroup->setExclusive(true);
			resetCoplanarity();
		}
		componentPresenceCallbackConnection.disconnect();
		startSelectionAct->setEnabled(true);
		stopSelectionAct->setEnabled(false);
		statusBar()->showMessage(tr("Checking presence of components stopped"));
		presence = true;
		selVis->setCurrentIndex(4);
		visualize( source_registered, "source_registered");

	}

}

void MainWindow::chooseCheck(int id){


	QString name = (bgroup->button(id))->text();

	if( name.compare("Presence") == 0 ){

		resetCoplanarity();
		presenceCheck();

	}

	if( name.compare("Coplanarity") == 0 ){

		angleTollerance->setEnabled(true);
		checkCoplanarity->setEnabled(true);
		resetPresence();
		selectComponent();

	}

	/*if((button->text()).compare("Radio button 1") == 0){

		QMessageBox* box = new QMessageBox();
		box->setWindowTitle("Messaggio");
		box->setText("Hai premuto " + button->text());
		box->show();

	}*/

}


void MainWindow::updateAngleTollerance(int threshold){
	angle = threshold;
}

void MainWindow::selectRibbonCable(){

	if(rib){
		componentCallbackCheckRibbonConnection = viewer->registerPointPickingCallback(&pointPickCallbackCheckRibbon, this);
		visualize( target_segmented, "target_segment" );
		ribbonCableAct->setIcon(QIcon("../icon/confirm.png"));
		statusBar()->setStatusTip("Confirm Selection? ");
		rib = false;
	}
	else{
		componentCallbackCheckRibbonConnection.disconnect();
		visualize( source_registered, "source_registered" );
		ribbonCableCheck();
		ribbonCableAct->setIcon(QIcon("../icon/ribbon.jpg"));
		rib = true;
	}

}

void MainWindow::ribbonCableCheck(){

	bool correct = isRibbonCableCorrect(source_registered);
	cout<<"Is Ribbon Cable correct? "<<correct<<endl;

}

void MainWindow::createActions(){

	openAct = new QAction(QIcon("../icon/open.png"), tr("&Open Source..."), this);
	openAct->setShortcut(tr("Ctrl+O"));
	openAct->setStatusTip(tr("Open a source file"));
	connect(openAct, SIGNAL(triggered()), this, SLOT(openFile()));

	openTargetAct = new QAction(QIcon("../icon/target.png"), tr("Open Target..."), this);
	openTargetAct->setShortcut(tr("Ctrl+T"));
	openTargetAct->setStatusTip(tr("Open a target file"));
	connect(openTargetAct, SIGNAL(triggered()), this, SLOT(openTarget()));

	saveAct = new QAction(QIcon("../icon/save.png"), tr("Sa&ve as..."), this);
	saveAct->setShortcut(tr("Ctrl+V"));
	saveAct->setStatusTip(tr("Save a component"));
	connect(saveAct, SIGNAL(triggered()), this, SLOT(newWindow()));

	segmentationAct = new QAction(QIcon("../icon/segmentation.png"), tr("&Segmentation..."), this);
	segmentationAct->setShortcut(tr("Ctrl+S"));
	segmentationAct->setStatusTip(tr("Run segmentation"));
	connect(segmentationAct, SIGNAL(triggered()), this, SLOT(segment()));

	registrationAct = new QAction(QIcon("../icon/registration.jpeg"), tr("&Registration..."), this);
	registrationAct->setShortcut(tr("Ctrl+R"));
	registrationAct->setStatusTip(tr("Run registration"));
	connect(registrationAct, SIGNAL(triggered()), this, SLOT(registers()));

	loadComponentAct = new QAction(QIcon("../icon/component.png"), tr("&Load Component..."), this);
	loadComponentAct->setShortcut(tr("Ctrl+L"));
	loadComponentAct->setStatusTip(tr("Load component"));
	connect(loadComponentAct, SIGNAL(triggered()), this, SLOT(loadComponent()));

	ribbonCableAct = new QAction(QIcon("../icon/ribbon.jpg"), tr("Select Ribbon Cable..."), this);
	ribbonCableAct->setStatusTip(tr("Select Ribbon Cable"));
	connect(ribbonCableAct, SIGNAL(triggered()), this, SLOT(selectRibbonCable()));

	settingAct = new QAction(QIcon("../icon/option.png"), tr("Options..."), this);
	settingAct->setStatusTip(tr("Options"));
	connect(settingAct, SIGNAL(triggered()), this, SLOT(options()));

	startSelectionAct = new QAction(QIcon("../icon/start.png"), tr("Start Presence Check..."), this);
	connect(startSelectionAct, SIGNAL(triggered()), this, SLOT(presenceCheck()));

	stopSelectionAct = new QAction(QIcon("../icon/stop.png"), tr("Stop Presence Check..."), this);
	connect(stopSelectionAct, SIGNAL(triggered()), this, SLOT(presenceCheck()));

	cableCheckAct = new QAction(QIcon("../icon/cable.png"), tr("Check Cable..."), this);
	cableCheckAct->setStatusTip(tr("Check if the cable is in the correct position"));
	connect(cableCheckAct, SIGNAL(triggered()), this, SLOT(cableCheck()));

	helpAct = new QAction(tr("&Help..."), this);
	helpAct->setStatusTip(tr("Help"));
	connect(helpAct, SIGNAL(triggered()), this, SLOT(about()));

	exitAct = new QAction(tr("E&xit..."), this);
	exitAct->setShortcut(tr("Ctrl+f"));
	exitAct->setStatusTip(tr("Exit from application"));
	connect(exitAct, SIGNAL(triggered()), this, SLOT(close()));

}

void MainWindow::createMenus(){

     fileMenu = menuBar()->addMenu(tr("&File"));
     fileMenu->addAction(openAct);
     fileMenu->addAction(openTargetAct);
     fileMenu->addAction(saveAct);
     fileMenu->addAction(exitAct);

     runMenu = menuBar()->addMenu(tr("&Run"));
     runMenu->addAction(segmentationAct);
     runMenu->addAction(registrationAct);
     runMenu->addSeparator();
     runMenu->addAction(loadComponentAct);
     runMenu->addAction(settingAct);

     checkMenu = menuBar()->addMenu(tr("&Check"));
     checkMenu->addAction(startSelectionAct);
     checkMenu->addAction(stopSelectionAct);
     checkMenu->addAction(cableCheckAct);
     checkMenu->addAction(ribbonCableAct);

     helpMenu = menuBar()->addMenu(tr("&Help"));
     helpMenu->addAction(helpAct);

}

void MainWindow::createToolBars(){

	fileToolBar = addToolBar(tr("File"));
	fileToolBar->addAction(openAct);
	fileToolBar->addAction(openTargetAct);
	fileToolBar->addAction(saveAct);

	runToolBar = addToolBar(tr("Run"));
	runToolBar->addAction(segmentationAct);
	runToolBar->addAction(registrationAct);
	runToolBar->addAction(loadComponentAct);

	checkToolBar = addToolBar(tr("Run"));
	checkToolBar->addAction(startSelectionAct);
	checkToolBar->addAction(stopSelectionAct);
	checkToolBar->addAction(cableCheckAct);
	checkToolBar->addAction(ribbonCableAct);

}

void MainWindow::createStatusBar(){
	statusBar()->showMessage(tr("Ready"));
}

void MainWindow::init(){

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr empty (new pcl::PointCloud<pcl::PointXYZRGB>);
	viewer->setBackgroundColor (0.5, 0.5, 0.5);
	viewer->addCoordinateSystem (0.1);
	viewer->initCameraParameters ();
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(empty);
	viewer->addPointCloud<pcl::PointXYZRGB> (empty, rgb, "empty_cloud");

	vtkSmartPointer<vtkRenderWindow> renderWindow = viewer->getRenderWindow();
	qvtkVisualizer->SetRenderWindow(renderWindow);
	viewer->setupInteractor(qvtkVisualizer->GetInteractor(), qvtkVisualizer->GetRenderWindow());

	viewer->getInteractorStyle()->setKeyboardModifier(pcl::visualization::INTERACTOR_KB_MOD_SHIFT);

	th_filter = FILTER_TH;
	th_seg = SEG_TH;

}

void MainWindow::sendConsole(QString s){

	//console->setText(console->toPlainText()+s);
	console->append(s);

	//necessario mettere l'if per poter disattivarlo quando si visualizza
	//per evitare incovenienti legati agli eventi quando si è in una callback
	if(cons)
		qApp->processEvents();

	QTextCursor c =  console->textCursor();
	c.movePosition(QTextCursor::End);
	console->setTextCursor(c);

}

void MainWindow::setCombobox(int ind, bool enabled){

	QStandardItemModel* model = qobject_cast<QStandardItemModel*>(selVis->model());
	if (model) {
		QModelIndex index = model->index(ind ,selVis->modelColumn(), selVis->rootModelIndex());
		QStandardItem* item = model->itemFromIndex(index);
		if (item) {
			item->setEnabled(enabled);
		}
	}

}


void MainWindow::pointPickCallback(const pcl::visualization::PointPickingEvent& event, void* cookie){

	MainWindow *m = (MainWindow*)cookie;

    float x,y,z;
    if (event.getPointIndex() == -1){
        m->statusBar()->showMessage(tr("No point was clicked"));
    	//m->sendConsole("No point was clicked \n");
    }
    else{

        event.getPoint(x,y,z);
        m->statusBar()->showMessage(QString("Point Clicked index: %1 x: %2 y: %3 z: %4")
                                 .arg(event.getPointIndex())
                                 .arg(x)
                                 .arg(y)
                                 .arg(z)
                                 );
        //m->sendConsole(QString("Point Clicked index: %1 x: %2 y: %3 z: %4 \n").arg(event.getPointIndex()).arg(x).arg(y).arg(z));

        pcl::copyPointCloud(*(source_registered), *(temp_target_segmented));
		pcl::PointIndices::Ptr clusterPoints(new pcl::PointIndices);
		segmentCluster(temp_target_segmented, clusterPoints, event.getPointIndex(), 2 );
		colorCluster (temp_target_segmented, clusterPoints, 0, 255, 0 );
		m->saveAct->setEnabled(true);
		m->selVis->setCurrentIndex(4);
		m->visualize(temp_target_segmented, "source_registered");

    }
}

void MainWindow::pointPickCallbackCheckPresence(const pcl::visualization::PointPickingEvent& event, void* cookie){

	MainWindow *m = (MainWindow*)cookie;

    float x,y,z;

    if (event.getPointIndex() != -1){

        event.getPoint(x,y,z);

        temp_target_segmented->clear();
        pcl::copyPointCloud(*(target_segmented), *(temp_target_segmented));
		pcl::PointIndices::Ptr clusterPoints(new pcl::PointIndices);
		segmentCluster(temp_target_segmented, clusterPoints, event.getPointIndex(), 2 );
		colorCluster (temp_target_segmented, clusterPoints, 0, 255, 0 );
		pcl::PointIndices::Ptr outIndices(new pcl::PointIndices);
		bool present = findCluster(target_segmented, source_registered, outIndices, event.getPointIndex(), 2 );
		m->selVis->setCurrentIndex(4);

		m->visualize(temp_target_segmented, "target_segmented");

		m->setConsole(false);

		if(present){
			m->sendConsole("The selected component is present \n");
			//VISUALIZZARE IL CORRISPONDENTE COMPONENTE NELLA SOURCE REGISTRATA
			temp_target_segmented->clear();
			m->resetPresence();
			pcl::copyPointCloud(*(source_registered), *(temp_target_segmented));
			colorCluster (temp_target_segmented, outIndices , 255, 0, 0 );
			m->visualize(temp_target_segmented, "source_registered");
		}
		else
			m->sendConsole("The selected component isn't present \n");

		m->setConsole(true);

    }
}

void MainWindow::pointPickCallbackCheckRibbon(const pcl::visualization::PointPickingEvent& event, void* cookie){

	MainWindow *m = (MainWindow*)cookie;

    if (event.getPointIndex() != -1){

        pcl::copyPointCloud(*(target_segmented), *(temp_target_segmented));
		pcl::PointIndices::Ptr clusterPoints(new pcl::PointIndices);
		segmentCluster(temp_target_segmented, clusterPoints, event.getPointIndex(), 2, true );
		colorCluster (temp_target_segmented, clusterPoints, 0, 255, 0 );

		Eigen::Vector4f centroid;
		pcl::compute3DCentroid(*(getRibbonCable()), centroid );
		float *data = centroid.data();
		cout<<"centroide trovato x: "<< data[0] <<" y: "<< data[1] <<" z: "<< data[2] <<endl;

		m->selVis->setCurrentIndex(4);
		m->visualize(temp_target_segmented, "target_segmented");

    }
}


void MainWindow::resetAll(){

	angle = COPLANARITY_TH;

	source_segmented->clear();
	source_registered->clear();
	temp_target_segmented->clear();

	selVis->setCurrentIndex(0);
	saveAct->setEnabled(false);

	checkCoplanarity->setEnabled(false);
	angleTollerance->setEnabled(false);
	angleTollerance->setValue(angle);

	if(bgroup->checkedButton() != 0){
		bgroup->setExclusive(false);
		bgroup->checkedButton()->setChecked(false);
		bgroup->setExclusive(true);
	}

	startSelectionAct->setEnabled(false);
	stopSelectionAct->setEnabled(false);
	presence = true;
	selection = false;
	rib = true;
	ribbonCableAct->setIcon(QIcon("../icon/ribbon.jpg"));
	componentCallbackConnection.disconnect();
	componentPresenceCallbackConnection.disconnect();

	console->clear();

}


void MainWindow::setConsole(bool c){
	cons = c;
}


void MainWindow::resetPresence(){

	componentPresenceCallbackConnection.disconnect();
	presence = true;
	startSelectionAct->setEnabled(true);
	stopSelectionAct->setEnabled(false);

	if(bgroup->checkedButton() != 0)
		if(bgroup->checkedButton()->text().compare("Presence") == 0){
			bgroup->setExclusive(false);
			bgroup->checkedButton()->setChecked(false);
			bgroup->setExclusive(true);
		}

}


void MainWindow::resetCoplanarity(){

	angle = COPLANARITY_TH;
	checkCoplanarity->setEnabled(false);
	angleTollerance->setEnabled(false);
	angleTollerance->setValue(angle);
	componentCallbackConnection.disconnect();
	selection = false;

	if(bgroup->checkedButton() != 0)
		if(bgroup->checkedButton()->text().compare("Coplanarity") == 0){
			bgroup->setExclusive(false);
			bgroup->checkedButton()->setChecked(false);
			bgroup->setExclusive(true);
		}

}

